#！/usr/bin/env python

import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

def rgbcallback(data):
    try:
        bridge = CvBridge()
        cv_image = bridge.imgmsg_to_cv2(data, 'rgb8')
        cv2.imshow('rgb', cv_image)
        cv2.waitKey(1)

    except:
        print(e)

def dcallback(data):
    try:
        bridge = CvBridge()
        cv_image = bridge.imgmsg_to_cv2(data, '32FC1')
        cv2.imshow('depth', cv_image)
        cv2.waitKey(1)

    except:
        print(e)

if __name__=="__main__":

    rospy.init_node('rgb_photo_node', anonymous = False)
    # rospy.init_node('d_photo_node',anonymous = False)
    rgb_topic = '/camera/rgb/image_raw'
    d_topic = '/camera/depth_registered/image_raw'
    rgb_sub = rospy.Subscriber(rgb_topic, Image, rgbcallback)
    d_sub = rospy.Subscriber(d_topic, Image, dcallback)
    rospy.sleep(1)
